package pathfinding

import (
	"math"
	"sort"
	"sync"
)

type multiPair struct {
	Area *MultiArea
	Node *MultiNode
}

type MultiJPS struct {
	Navigable map[int16]map[int16]int
	Areas     []*MultiArea
	NodeMap   map[Vector2]*MultiNode
	Nodes     []Vector2
	Instance  JPSInterface
	Dijkstra  *Dijkstra

	resultQueue    sync.Pool
	jpsQueue       sync.Pool
	multiQueue     sync.Pool
	multiPairQueue sync.Pool
}

func NewMultiJPS(instance JPSInterface, navigable map[int16]map[int16]int) *MultiJPS {
	mjps := &MultiJPS{
		Navigable: navigable,
		Areas:     []*MultiArea{},
		NodeMap:   map[Vector2]*MultiNode{},
		Nodes:     []Vector2{},
		Instance:  instance,
		Dijkstra:  NewDijkstra(),
		resultQueue: sync.Pool{New: func() interface{} {
			return []Vector2{}
		}},
		jpsQueue: sync.Pool{New: func() interface{} {
			return NewJPS(instance)
		}},
		multiQueue: sync.Pool{New: func() interface{} {
			return []DijkstraNode{}
		}},
		multiPairQueue: sync.Pool{New: func() interface{} {
			return []*multiPair{}
		}},
	}
	return mjps
}

func (mjps *MultiJPS) getIdentity(point Vector2) int {
	var yy map[int16]int
	if yyy, ok := mjps.Navigable[point.X]; !ok {
		yy = map[int16]int{}
		mjps.Navigable[point.X] = yy
	} else {
		yy = yyy
	}
	if v, ok := yy[point.Y]; ok {
		return v
	} else {
		return 0
	}
}

func (mjps *MultiJPS) setIdentity(point *Vector2, identity int) {
	var yy map[int16]int
	if yyy, ok := mjps.Navigable[point.X]; !ok {
		yy = map[int16]int{}
		mjps.Navigable[point.X] = yy
	} else {
		yy = yyy
	}
	yy[point.Y] = identity
}

func (mjps *MultiJPS) AddArea(polygon []Vector2, points []Vector2, t byte) *MultiArea {
	area := NewMultiArea(polygon, mjps.Navigable)
	mjps.Areas = append(mjps.Areas, area)

	for _, p := range points {
		var node *MultiNode
		if n, ok := mjps.NodeMap[p]; !ok {
			node = &MultiNode{
				LinkNode: &LinkNode{
					Grid:        area.Grid,
					Type:        t,
					Identity:    mjps.getIdentity(p),
					Point:       p,
					Connections: []DijkstraEdge{},
				},
				Areas: make([]*MultiArea, 2),
			}
			mjps.NodeMap[p] = node
			mjps.Nodes = append(mjps.Nodes, p)
		} else {
			node = n
		}
		for _, n := range area.Nodes {
			node.ConnectDuplexSingle(n.LinkNode)
		}
		area.Nodes = append(area.Nodes, node)
		node.Bind(area)
	}
	return area
}

func (mjps *MultiJPS) UpdateNode(point *Vector2, identity int, t byte) {
	mjps.setIdentity(point, identity)
	for _, area := range mjps.Areas {
		if area.Contains(point) {
			area.Tailor(point)
			area.Grid.UpdateNode(point, identity, t)
		}
	}
}

func (mjps *MultiJPS) FindPath(start *Vector2, end *Vector2, mask PathMask) (bool, []Vector2) {
	f := 0
	var startArea *MultiArea
	var endArea *MultiArea
	r := false
	for _, area := range mjps.Areas {
		if f&1 == 0 && area.Contains(start) {
			startArea = area
			f |= 1
		}
		if f&2 == 0 && area.Contains(end) {
			endArea = area
			f |= 2
		}
		if f == 3 {
			break
		}
	}
	if f != 3 || startArea == nil || endArea == nil {
		return r, nil
	}
	result := mjps.resultQueue.Get().([]Vector2)
	result = result[0:0]
	if startArea == endArea {
		r, result = mjps.SimpleFindPath(startArea, start, end, mask, result)
	} else {
		r, result = mjps.MultiFindPath(startArea, endArea, start, end, mask, result)
	}
	return r, result
}

func (mjps *MultiJPS) SimpleFindPath(area *MultiArea, start *Vector2, end *Vector2, mask PathMask, result []Vector2) (bool, []Vector2) {
	jps := mjps.jpsQueue.Get().(*JPS)
	area.Tailor(start)
	area.Tailor(end)
	r, result := jps.FindPath(area.Grid, start, end, mask, false, result)
	if r {
		area.Restore(result)
	}
	mjps.jpsQueue.Put(jps)
	return r, result
}

func (mjps *MultiJPS) MultiFindPath(startArea *MultiArea, endArea *MultiArea, start *Vector2, end *Vector2, mask PathMask, result []Vector2) (bool, []Vector2) {
	rr := mjps.multiPairQueue.Get().([]*multiPair)
	rr = rr[0:0]
	r, rr := mjps.NodePath(startArea, endArea, start, end, mask, rr)
	var rs bool
	if r {
		rrr := mjps.resultQueue.Get().([]Vector2)
		for _, pair := range rr {
			rrr = rrr[0:0]
			rs, result = mjps.SimpleFindPath(pair.Area, start, &pair.Node.Point, mask, rrr)
			if !rs {
				return false, result
			}
			start = &pair.Node.Point
			result = append(result, rrr...)
		}
		mjps.resultQueue.Put(rrr)
	}
	mjps.multiPairQueue.Put(rr)
	return r, result
}

func (mjps *MultiJPS) NodePath(startArea *MultiArea, endArea *MultiArea, start *Vector2, end *Vector2, mask PathMask, result []*multiPair) (bool, []*multiPair) {
	rr := mjps.multiQueue.Get().([]DijkstraNode)
	rr = rr[0:0]
	startNode := &MultiNode{
		LinkNode: &LinkNode{
			Grid:  startArea.Grid,
			Point: *start,
		},
		Areas: make([]*MultiArea, 2),
	}
	startNode.Bind(startArea)
	for _, n := range startArea.Nodes {
		startNode.ConnectSimplexSingle(n.LinkNode)
	}
	endNode := &MultiNode{
		LinkNode: &LinkNode{
			Grid:  endArea.Grid,
			Point: *end,
		},
		Areas: make([]*MultiArea, 2),
	}
	endNode.Bind(endArea)
	// dijkstra内部自动将edges反向连接
	for _, n := range endArea.Nodes {
		endNode.ConnectSimplexSingle(n.LinkNode)
	}
	r, _ := mjps.Dijkstra.ToDijkstra(startNode.LinkNode, endNode.LinkNode, mask, &rr, RouteLogicInf)
	if r {
		for index, n := range rr {
			if index == 0 {
				continue
			}
			nn := n.(*LinkNode)
			v := nn.Point
			node := mjps.NodeMap[v]
			if node == nil && v.Equals(end) {
				node = endNode
			}
			//fmt.Println(1, index, n.Point)
			//fmt.Println(2, node.Areas)
			//fmt.Println(3, *node.Areas[0])
			result = append(result, &multiPair{
				Area: startArea,
				Node: node,
			})
			if node.Areas[0] == startArea {
				startArea = node.Areas[1]
			} else {
				startArea = node.Areas[0]
			}
		}
	}
	mjps.multiQueue.Put(rr)
	return r, result
}

type MultiNode struct {
	*LinkNode
	Areas []*MultiArea
}

func (node *MultiNode) Bind(area *MultiArea) {
	if node.Areas[0] == nil {
		node.Areas[0] = area
	} else if node.Areas[1] == nil {
		node.Areas[1] = area
	} else {

	}
}

type MultiArea struct {
	Grid    JPSGrid
	Start   Vector2
	Polygon []Vector2
	Nodes   []*MultiNode
}

func NewMultiArea(Polygon []Vector2, navigable map[int16]map[int16]int) *MultiArea {
	minX := int16(math.MaxInt16)
	minY := int16(math.MaxInt16)
	maxX := int16(math.MinInt16)
	maxY := int16(math.MinInt16)
	for i := 0; i < len(Polygon); i++ {
		p := Polygon[i]
		minX = int16(math.Min(float64(minX), float64(p.X)))
		minY = int16(math.Min(float64(minY), float64(p.Y)))
		maxX = int16(math.Max(float64(maxX), float64(p.X)))
		maxY = int16(math.Max(float64(maxY), float64(p.Y)))
	}
	start := Vector2{
		X: minX,
		Y: minY,
	}
	nodes := []*MultiNode{}
	n := make([][]byte, maxX-minX+1)
	for i := int16(0); i <= maxX-minX; i++ {
		n[i] = make([]byte, maxY-minY+1)
		var nav map[int16]int
		if nn, ok := navigable[i+minX]; !ok {
			continue
		} else {
			nav = nn
		}
		for j := int16(0); j <= maxY-minY; j++ {
			if navnav, ok := nav[j+minY]; !ok {
				continue
			} else {
				n[i][j] = byte(navnav)
			}
		}
	}
	nav, members := TransformGrid(n)
	area := &MultiArea{
		Grid:    NewGrid(start, nav, members),
		Start:   start,
		Polygon: Polygon,
		Nodes:   nodes,
	}

	return area
}

func (area *MultiArea) Contains(p *Vector2) bool {
	crossNum := 0
	vertexCount := len(area.Polygon)
	for i := 0; i < vertexCount; i++ {
		v1 := area.Polygon[i]
		v2 := area.Polygon[(i+1)%vertexCount]

		if ((v1.Y <= p.Y) && (v2.Y > p.Y)) || ((v1.Y > p.Y) && (v2.Y <= p.Y)) {
			if p.X < v1.X+(p.Y-v1.Y)/(v2.Y-v1.Y)*(v2.X-v1.X) {
				crossNum += 1
			}
		}
	}

	return crossNum%2 != 0
}

func (area *MultiArea) Tailor(point *Vector2) {
	point.X -= area.Start.X
	point.Y -= area.Start.Y
}

func (area *MultiArea) Restore(nodes []Vector2) {
	for i, _ := range nodes {
		nodes[i].X += area.Start.X
		nodes[i].Y += area.Start.Y
	}
}

type LinkNode struct {
	Grid        JPSGrid
	Point       Vector2
	Type        byte
	Identity    int
	Connections []DijkstraEdge
}

func (current *LinkNode) Distance(end DijkstraNode) float64 {
	e := end.(*LinkNode)
	return math.Sqrt(math.Pow(float64(current.Point.X)-float64(e.Point.X), 2) + math.Pow(float64(current.Point.Y)-float64(e.Point.Y), 2))
}

func (current *LinkNode) Exist() bool {
	return current != nil
}

func (current *LinkNode) GetIdentity() int {
	return current.Identity
}

func (current *LinkNode) GetType() byte {
	return current.Type
}

func (current *LinkNode) GetItem() []byte {
	return nil
}

func (current *LinkNode) GetSeat() *Seat {
	return nil
}

func (current *LinkNode) GetFixed() *Fixed {
	return nil
}

func (current *LinkNode) Can(mask PathMask) bool {
	// 不会执行
	return mask.CanPath(nil, current)
}

func (current *LinkNode) GetId() interface{} {
	return &current.Point
}

func (current *LinkNode) GetEdges(endLink map[interface{}]DijkstraEdge, mask PathMask) []DijkstraEdge {
	if edge, ok := endLink[current.GetId()]; ok {
		return append(current.Connections, edge)
	}
	return current.Connections
}

func (current *LinkNode) EqualNode(node *LinkNode) bool {
	return current.Point.Equals(&node.Point)
}

func (current *LinkNode) Equal(other interface{}) bool {
	return current.Point.Equals(&other.(*LinkNode).Point)
}

func (current *LinkNode) ConnectDuplex(nodes []*LinkNode) {
	connections := []*Edge{}
	for _, node := range nodes {
		if node.Equal(current) {
			continue
		}
		dist := math.Abs(float64(current.Point.X-node.Point.X)) + math.Abs(float64(current.Point.Y-node.Point.Y))
		connections = append(connections, &Edge{
			Cost:        dist,
			ConnectNode: node,
		})
	}
	sort.Slice(connections, func(i, j int) bool {
		return connections[i].Cost > connections[j].Cost
	})
	for _, cnn := range connections {
		flag := true
		for _, edge := range current.Connections {
			if current.Equal(edge.Connect()) {
				flag = false
				break
			}
		}
		if flag {
			current.Connections = append(current.Connections, cnn)
		}
		flag = true
		for _, edge := range cnn.ConnectNode.Connections {
			if current.Equal(edge.Connect()) {
				flag = false
			}
		}
		if flag {
			cnn.ConnectNode.Connections = append(cnn.ConnectNode.Connections, &Edge{
				Cost:        cnn.Cost,
				ConnectNode: current,
			})

			sort.Slice(cnn.ConnectNode.Connections, func(i, j int) bool {
				return cnn.ConnectNode.Connections[i].GetCost() > cnn.ConnectNode.Connections[j].GetCost()
			})
		}
	}
}

func (current *LinkNode) ConnectSimplex(nodes []*LinkNode) {
	for _, node := range nodes {
		if node.Equal(current) {
			continue
		}
		dist := math.Abs(float64(current.Point.X-node.Point.X)) + math.Abs(float64(current.Point.Y-node.Point.Y))
		current.Connections = append(current.Connections, &Edge{
			Cost:        dist,
			ConnectNode: node,
		})
	}
	sort.Slice(current.Connections, func(i, j int) bool {
		return current.Connections[i].GetCost() > current.Connections[j].GetCost()
	})
}

func (current *LinkNode) ConnectDuplexSingle(node *LinkNode) {
	dist := math.Abs(float64(current.Point.X-node.Point.X)) + math.Abs(float64(current.Point.Y-node.Point.Y))
	current.Connections = append(current.Connections, &Edge{
		Cost:        dist,
		ConnectNode: node,
	})
	flag := true
	for _, edge := range node.Connections {
		if current.Equal(edge.Connect()) {
			flag = false
		}
	}
	if flag {
		node.Connections = append(node.Connections, &Edge{
			Cost:        dist,
			ConnectNode: current,
		})
		sort.Slice(node.Connections, func(i, j int) bool {
			return node.Connections[i].GetCost() > node.Connections[j].GetCost()
		})
	}
}

func (current *LinkNode) ConnectSimplexSingle(node *LinkNode) {
	if node.Equal(current) {
		return
	}
	dist := math.Abs(float64(current.Point.X-node.Point.X)) + math.Abs(float64(current.Point.Y-node.Point.Y))
	current.Connections = append(current.Connections, &Edge{
		Cost:        dist,
		ConnectNode: node,
	})
	sort.Slice(current.Connections, func(i, j int) bool {
		return current.Connections[i].GetCost() > current.Connections[j].GetCost()
	})
}

func (current *LinkNode) DisconnectSimplex(node *LinkNode) {
	index := -1
	for i, cnn := range current.Connections {
		if cnn.Connect().Equal(node) {
			index = i
			break
		}
	}
	if index >= 0 {
		if index == 0 {
			current.Connections = current.Connections[1:]
		} else if index == len(current.Connections)-1 {
			current.Connections = current.Connections[:index]
		} else {
			current.Connections = append(current.Connections[:index], current.Connections[index+1:]...)
		}
	}
}

func (current *LinkNode) DisconnectDuplex(nodes []LinkNode) {
	for _, node := range nodes {
		node.DisconnectSimplex(current)
		current.DisconnectSimplex(&node)
	}
}

type Edge struct {
	Cost        float64
	ConnectNode *LinkNode
}

func (edge *Edge) GetCost() float64 {
	return edge.Cost
}

func (edge *Edge) Connect() DijkstraNode {
	return edge.ConnectNode
}
func (edge *Edge) Switch(node DijkstraNode) {
	edge.ConnectNode = node.(*LinkNode)
}
